
using System;
using System.Diagnostics;

namespace Dida
{
	public class Matrix4 : ICloneable
	{
		public Vector4D[] rows;

		/// <summary>
		/// Creates a Matrix4 with all it's components set to 0.
		/// </summary>
		/// <seealso cref="Identity"/>
		public Matrix4()
		{
			rows = new Vector4D[4];
			rows[0] = new Vector4D();
			rows[1] = new Vector4D();
			rows[2] = new Vector4D();
			rows[3] = new Vector4D();
		}

		/// <summary>
		/// Creates a matrix from an array of 16 floats. The floats are expected
		/// to be row major, which means that the first 4 floats form the base
		/// vector for the x axis. Note that this is different from what OpenGL
		/// and DirectX normally use, so if you want to convert from a float
		/// array you got from one of those APIs you'll have to use the
		/// <see cref = "FromTransposedFloatArray">FromTransposedFloatArray</see>
		/// method.
		/// </summary>
		/// <param name="floats">An array of 16 floats, that form a matrix.</param>
		/// <returns>The matrix that was created from the given float array</returns>
		public static Matrix4 FromFloatArray(float[] floats)
		{
			Matrix4 ret = new Matrix4();
			int readPtr = 0;
			for (int i = 0; i < 4; i++)
				for (int j = 0; j < 4; j++)
					ret.rows[i][j] = floats[readPtr++];
			return ret;
		}

		/// <summary>
		/// Creates a matrix from an array of 16 floats. The floats are expected
		/// to be column major. This differs from the representation of this
		/// Matrix4 class, since it is row major, but is what's used
		/// by APIs such as DirectX and OpenGL, so if you want to 
		/// create a matrix from an array of floats you got from
		/// those API's, this is the method you want to use.
		/// </summary>
		/// <param name="floats">An array of 16 floats, that form a matrix.</param>
		/// <returns>The matrix that was created from the given float array</returns>
		public static Matrix4 FromTransposedFloatArray(float[] floats)
		{
			Matrix4 ret = new Matrix4();
			int readPtr = 0;
			for (int i = 0; i < 4; i++)
				for (int j = 0; j < 4; j++)
					ret.rows[j][i] = floats[readPtr++];
			return ret;
		}

		/// <summary>
		/// Gets the Identity matrix.
		/// </summary>
		public static readonly Matrix4 Identity = GetIdentity();
		private static Matrix4 GetIdentity()
		{
			Matrix4 ret = new Matrix4();
			ret.rows[0].Set(1, 0, 0, 0);
			ret.rows[1].Set(0, 1, 0, 0);
			ret.rows[2].Set(0, 0, 1, 0);
			ret.rows[3].Set(0, 0, 0, 1);
			return ret;
		}

		/// <summary>
		/// Returns a matrix that contains the given translation.
		/// </summary>
		/// <param name="v">The vector by which to translate.</param>
		/// <returns>The matrix containing the given translation.</returns>
		public static Matrix4 GetTranslationMatrix(Vector3D v)
		{
			Matrix4 ret = new Matrix4();
			ret.rows[0].Set(1, 0, 0, v.x);
			ret.rows[1].Set(0, 1, 0, v.y);
			ret.rows[2].Set(0, 0, 1, v.z);
			ret.rows[3].Set(0, 0, 0, 1);
			return ret;
		}

		/// <summary>
		/// Returns a matrix that contains the given translation.
		/// </summary>
		/// <param name="x">The x component of the translation vector.</param>
		/// <param name="y">The y component of the translation vector.</param>
		/// <param name="z">The z component of the translation vector.</param>
		/// <returns>The matrix containing the given translation.</returns>
		public static Matrix4 GetTranslationMatrix(float x, float y, float z)
		{
			return GetTranslationMatrix(new Vector3D(x, y, z));
		}

		/// <summary>
		/// <para>
		/// Returns a matrix that contains the given yaw rotation.
		/// </para>
		/// <para>
		/// The yaw rotation is about the y axis. If looked in the positive y direction,
		/// the rotation is in counter clockwise direction.
		/// </para>
		/// </summary>
		/// <param name="angle">the angle to rotate, in radians</param>
		/// <returns>The matrix containing the given yaw rotation</returns>
		public static Matrix4 GetYawMatrix(float angle)
		{
			float s = (float)Math.Sin(angle);
			float c = (float)Math.Cos(angle);

			Matrix4 ret = new Matrix4();
			ret.rows[0].Set(c, 0, -s, 0);
			ret.rows[1].Set(0, 1, 0, 0);
			ret.rows[2].Set(s, 0, c, 0);
			ret.rows[3].Set(0, 0, 0, 1);
			return ret;
		}

		/// <summary>
		/// <para>
		/// Returns a matrix that contains the given pitch rotation.
		/// </para>
		/// <para>
		/// The yaw rotation is about the x axis. If looked in the positive x direction,
		/// the rotation is in counter clockwise direction.
		/// </para>
		/// </summary>
		/// <param name="angle">the angle to rotate, in radians</param>
		/// <returns>The matrix containing the given pitch rotation</returns>
		public static Matrix4 GetPitchMatrix(float angle)
		{
			float s = (float)Math.Sin(angle);
			float c = (float)Math.Cos(angle);

			Matrix4 ret = new Matrix4();
			ret.rows[0].Set(1, 0, 0, 0);
			ret.rows[1].Set(0, c, s, 0);
			ret.rows[2].Set(0, -s, c, 0);
			ret.rows[3].Set(0, 0, 0, 1);
			return ret;
		}

		/// <summary>
		/// <para>
		/// Returns a matrix that contains the given roll rotation.
		/// </para>
		/// <para>
		/// The yaw rotation is about the z axis. If looked in the positive z direction,
		/// the rotation is in counter clockwise direction.
		/// </para>
		/// </summary>
		/// <param name="angle">the angle to rotate, in radians</param>
		/// <returns>The matrix containing the given roll rotation</returns>
		public static Matrix4 GetRollMatrix(float angle)
		{
			float s = (float)Math.Sin(angle);
			float c = (float)Math.Cos(angle);

			Matrix4 ret = new Matrix4();
			ret.rows[0].Set(c, -s, 0, 0);
			ret.rows[1].Set(s, c, 0, 0);
			ret.rows[2].Set(0, 0, 1, 0);
			ret.rows[3].Set(0, 0, 0, 1);
			return ret;
		}

		public static Matrix4 GetScaleMatrix(Vector3D v)
		{
			Matrix4 ret = new Matrix4();
			ret.rows[0].Set(v.x, 0, 0, 0);
			ret.rows[1].Set(0, v.y, 0, 0);
			ret.rows[2].Set(0, 0, v.z, 0);
			ret.rows[3].Set(0, 0, 0, 1);
			return ret;
		}

		public static Matrix4 GetScaleMatrix(float x, float y, float z)
		{
			Matrix4 ret = new Matrix4();
			ret.rows[0].Set(x, 0, 0, 0);
			ret.rows[1].Set(0, y, 0, 0);
			ret.rows[2].Set(0, 0, z, 0);
			ret.rows[3].Set(0, 0, 0, 1);
			return ret;
		}
		
		/// <summary>
		/// <para>Retuns a left handed projection matrix.</para>
		/// <para>A left handed projection matrix projects a 3D coordinate system
		/// with the x to the right, the y up, and the z away from you, 
		/// to a 2D coordinate system, with the bottomleft corner of the 
		/// view at (-1,-1) and the topright corner of the view at (1,1)
		/// </para>
		/// </summary>
		/// <param name="yFov">The angle of the field of view, in the y direction</param>
		/// <param name="aspect">The ration between the view width and the
		/// view height. For a normal screen, this should be 4 / 3, for widescreen
		/// this should be 16 / 9</param>
		/// <param name="nearPlane">the distance of the near clip plane</param>
		/// <param name="farPlane">the distance of the far clip plane</param>
		/// <returns>The newly created projection matrix</returns>
		public static Matrix4 GetProjectionMatrix(float yFov, float aspect,
			float nearPlane, float farPlane)
		{
			float halfFov = yFov * .5f;
			float yScale = (float)(1 / Math.Tan(halfFov));
			float xScale = yScale / aspect;

			float Q = farPlane / (farPlane - nearPlane);

			Matrix4 ret = new Matrix4();
			ret.rows[0].Set(xScale, 0, 0, 0);
			ret.rows[1].Set(0, yScale, 0, 0);
			ret.rows[2].Set(0, 0, Q, -Q * nearPlane);
			ret.rows[3].Set(0, 0, 1, 0);
			return ret;
		}

		/// <summary>
		/// Multiplies two matrices.
		/// </summary>
		/// <remarks>
		/// Multiplying two matrices gives you a matrix that combines the
		/// effect of matrix a and b. Transforming a point by this matrix
		/// has the same effect as first transforming the point by matrix a
		/// and then transforming it by matrix b.
		/// </remarks>
		/// <param name="a">The first operand.</param>
		/// <param name="b">The second operand.</param>
		/// <returns>The result of the multiplication.</returns>
		public static Matrix4 operator *(Matrix4 a, Matrix4 b)
		{
			Matrix4 ret = new Matrix4();
			for (int i = 0; i < 4; i++)
			{
				for (int j = 0; j < 4; j++)
				{
					ret.rows[i][j] =
						a.rows[0][j] * b.rows[i][0] +
						a.rows[1][j] * b.rows[i][1] +
						a.rows[2][j] * b.rows[i][2] +
						a.rows[3][j] * b.rows[i][3];
				}
			}
			return ret;
		}

		private void MultiplyWith(Matrix4 b)
		{
			Vector4D[] newRows = new Vector4D[4];
			for (int i = 0; i < 4; i++)
			{
				for (int j = 0; j < 4; j++)
				{
					newRows[i][j] =
						rows[0][j] * b.rows[i][0] +
						rows[1][j] * b.rows[i][1] +
						rows[2][j] * b.rows[i][2] +
						rows[3][j] * b.rows[i][3];
				}
			}
			rows = newRows;
		}

		/// <summary>
		/// Applies a translation to this matrix, on top of the current
		/// transformations. It has the same effect as multiplying this matrix 
		/// with a matrix that has a transformation that was retrieved
		/// using the <see cref = "GetTranslationMatrix(float,float,float)">GetTranslationMatrix
		/// </see> method.
		/// </summary>
		/// <seealso cref="Translate(Vector3D)"/>
		/// <param name="x">the x part of the vector you want to translate by</param>
		/// <param name="y">the y part of the vector you want to translate by</param>
		/// <param name="z">the z part of the vector you want to translate by</param>
		public void Translate(float x, float y, float z)
		{
			float[] vec = {x,y,z,0};
			for(int row = 0;row < 4;row++)
			{
				for(int col = 0;col < 4;col++)
				{
					rows[row][col] += rows[3][col] * vec[row];
				}
			}
		}

		/// <summary>
		/// Applies a translation to this matrix, on top of the current
		/// transformations. It has the same effect as multiplying this matrix 
		/// with a matrix that has a transformation that was retrieved
		/// using the <see cref = "GetTranslationMatrix(Vector3D)">
		/// GetTranslationMatrix</see> method.
		/// </summary>
		/// <seealso cref="Translate(float,float,float)"/>
		/// <seealso cref="GetTranslationMatrix(Vector3D)"/>
		/// <param name="v">the vector you want to translate by</param>
		public void Translate(Vector3D v)
		{
			Translate(v.x, v.y, v.z);
		}

		public void RotateYaw(float angle)
		{
			MultiplyWith(GetYawMatrix(angle));
		}

		public void RotatePitch(float angle)
		{
			MultiplyWith(GetPitchMatrix(angle));
		}

		public void RotateRoll(float angle)
		{
			MultiplyWith(GetRollMatrix(angle));
		}

		public void Transpose()
		{
			for (int i = 0; i < 4; i++)
			{
				for (int j = i + 1; j < 4; j++)
				{
					float tmp = rows[i][j];
					rows[i][j] = rows[j][i];
					rows[j][i] = tmp;
				}
			}
		}

		public Matrix4 GetTransposed()
		{
			Matrix4 ret = new Matrix4();
			for(int i = 0;i < 4;i++)
				for (int j = 0; j < 4; j++)
				{
					ret.rows[i][j] = rows[j][i];
				}
			return ret;
		}

		/// <summary>
		/// Returns the inverse of this matrix.
		/// </summary>
		/// <returns>The inverse of this matrix</returns>
		/// <seealso cref="Invert()"/>
		public Matrix4 GetInversed()
		{
			// TODO: Matrix GetInversed method is ripped from ogre. has to be rewritten.

			float m00 = rows[0].x, m01 = rows[0].y, m02 = rows[0].z, m03 = rows[0].w;
			float m10 = rows[1].x, m11 = rows[1].y, m12 = rows[1].z, m13 = rows[1].w;
			float m20 = rows[2].x, m21 = rows[2].y, m22 = rows[2].z, m23 = rows[2].w;
			float m30 = rows[3].x, m31 = rows[3].y, m32 = rows[3].z, m33 = rows[3].w;

			float v0 = m20 * m31 - m21 * m30;
			float v1 = m20 * m32 - m22 * m30;
			float v2 = m20 * m33 - m23 * m30;
			float v3 = m21 * m32 - m22 * m31;
			float v4 = m21 * m33 - m23 * m31;
			float v5 = m22 * m33 - m23 * m32;

			float t00 = + (v5 * m11 - v4 * m12 + v3 * m13);
			float t10 = - (v5 * m10 - v2 * m12 + v1 * m13);
			float t20 = + (v4 * m10 - v2 * m11 + v0 * m13);
			float t30 = - (v3 * m10 - v1 * m11 + v0 * m12);

			float invDet = 1 / (t00 * m00 + t10 * m01 + t20 * m02 + t30 * m03);

			float d00 = t00 * invDet;
			float d10 = t10 * invDet;
			float d20 = t20 * invDet;
			float d30 = t30 * invDet;

			float d01 = - (v5 * m01 - v4 * m02 + v3 * m03) * invDet;
			float d11 = + (v5 * m00 - v2 * m02 + v1 * m03) * invDet;
			float d21 = - (v4 * m00 - v2 * m01 + v0 * m03) * invDet;
			float d31 = + (v3 * m00 - v1 * m01 + v0 * m02) * invDet;

			v0 = m10 * m31 - m11 * m30;
			v1 = m10 * m32 - m12 * m30;
			v2 = m10 * m33 - m13 * m30;
			v3 = m11 * m32 - m12 * m31;
			v4 = m11 * m33 - m13 * m31;
			v5 = m12 * m33 - m13 * m32;

			float d02 = + (v5 * m01 - v4 * m02 + v3 * m03) * invDet;
			float d12 = - (v5 * m00 - v2 * m02 + v1 * m03) * invDet;
			float d22 = + (v4 * m00 - v2 * m01 + v0 * m03) * invDet;
			float d32 = - (v3 * m00 - v1 * m01 + v0 * m02) * invDet;

			v0 = m21 * m10 - m20 * m11;
			v1 = m22 * m10 - m20 * m12;
			v2 = m23 * m10 - m20 * m13;
			v3 = m22 * m11 - m21 * m12;
			v4 = m23 * m11 - m21 * m13;
			v5 = m23 * m12 - m22 * m13;
			  
			float d03 = - (v5 * m01 - v4 * m02 + v3 * m03) * invDet;
			float d13 = + (v5 * m00 - v2 * m02 + v1 * m03) * invDet;
			float d23 = - (v4 * m00 - v2 * m01 + v0 * m03) * invDet;
			float d33 = + (v3 * m00 - v1 * m01 + v0 * m02) * invDet;
			  
			Matrix4 ret = new Matrix4();
			ret.rows[0].Set(d00, d01, d02, d03);
			ret.rows[1].Set(d10, d11, d12, d13);
			ret.rows[2].Set(d20, d21, d22, d23);
			ret.rows[3].Set(d30, d31, d32, d33);
			return ret;
		}

		/// <summary>
		/// Inverts this matrix.
		/// </summary>
		/// <seealso cref = "GetInversed()"/>
		public void Invert()
		{
			rows = GetInversed().rows;
		}

		/// <summary>
		/// Transforms the given point by the first three rows if the matrix.
		/// The W part is simply ignored. This should be convenient for 
		/// normal transformations matrices (rotation, translation, etc.).
		/// For projection matrices, you should use
		/// <see cref = "TransformAndProject"/> or
		/// <see cref = "TransformHomogeneous"/> method.
		/// </summary>
		/// <param name="pt">The point you want to transform.</param>
		/// <returns>The transformed version of pt</returns>
		public Vector3D Transform(Vector3D pt)
		{
			return new Vector3D(
				pt.x * rows[0].x + pt.y * rows[0].y + pt.z * rows[0].z + rows[0].w,
				pt.x * rows[1].x + pt.y * rows[1].y + pt.z * rows[1].z + rows[1].w,
				pt.x * rows[2].x + pt.y * rows[2].y + pt.z * rows[2].z + rows[2].w);
		}

		/// <summary>
		/// Transforms the given vector by this matrix, including the homogeneous
		/// part, and projects the resulting homogeneous vector to 3D by 
		/// performing the w divide.
		/// </summary>
		/// <param name="pt"></param>
		/// <returns>The transformed and projected vector. If this
		/// is a projection matrix, this is the vector in screen space.</returns>
		/// <remarks>
		/// <para>
		/// The given 3D vector is extended to a 4D homogeneous vector, with a 
		/// W component of 1, then it's transformed by this matrix, and finally
		/// the vector is reduced to a 3D vector, by dividing all components by W.
		/// </para>
		/// <para>
		/// If this matrix is a projection matrix, the x and y components of the result can
		/// be used as screen coordinates directly, and the z coordinate can be
		/// used in the z buffer
		/// </para>
		/// </remarks>
		public Vector3D TransformAndProject(Vector3D pt)
		{
			Vector4D res = new Vector4D(
				pt.x * rows[0].x + pt.y * rows[0].y + pt.z * rows[0].z + rows[0].w,
				pt.x * rows[1].x + pt.y * rows[1].y + pt.z * rows[1].z + rows[1].w,
				pt.x * rows[2].x + pt.y * rows[2].y + pt.z * rows[2].z + rows[2].w,
				pt.x * rows[3].x + pt.y * rows[3].y + pt.z * rows[3].z + rows[3].w);
			
			float reciW = 1 / res.w;
			return new Vector3D(res.x * reciW, res.y * reciW, res.z * reciW);
		}

		/// <summary>
		/// Transforms the given vector, and returns it as a 4D homogeneous vector.
		/// </summary>
		/// <param name="pt"></param>
		/// <returns></returns>
		/// <remarks>
		/// <para>
		/// This method extends the given <see cref = "Vector3D">3D vector</see>
		/// with a w component of 1, transforms that homogeneous vector, 
		/// and returns it as a <see cref = "Vector3D">4D vector</see>.
		/// </para>
		/// <para>
		/// For matrices that do simple transformations, like translation, rotation and
		/// scaling, the <see cref = "Transform"/> method should be sufficent.
		/// This function should be used when the matrix (may) contain 
		/// projection transformations.
		/// </para>
		/// </remarks>
		/// <seealso cref="Transform"/>
		/// <seealso cref="TransformAndProject"/>
		public Vector4D TransformHomogeneous(Vector3D pt)
		{
			return new Vector4D(
				pt.x * rows[0].x + pt.y * rows[0].y + pt.z * rows[0].z + rows[0].w,
				pt.x * rows[1].x + pt.y * rows[1].y + pt.z * rows[1].z + rows[1].w,
				pt.x * rows[2].x + pt.y * rows[2].y + pt.z * rows[2].z + rows[2].w,
				pt.x * rows[3].x + pt.y * rows[3].y + pt.z * rows[3].z + rows[3].w);
		}

		public float[] ToFloatArray()
		{
			float[] ret = new float[16];
			int writePtr = 0;
			for(int i = 0;i < 4;i++)
				for (int j = 0; j < 4; j++)
					ret[writePtr++] = rows[i][j];

			return ret;
		}

		public float[] ToTransposedFloatArray()
		{
			float[] ret = new float[16];
			int writePtr = 0;
			for (int i = 0; i < 4; i++)
				for (int j = 0; j < 4; j++)
					ret[writePtr++] = rows[j][i];

			return ret;
		}

		public void SetBaseVector(int i, Vector3D v)
		{
			Debug.Assert(i >= 0 && i < 3, "You can only set the 3x3 part of the matrix with the SetBaseVector(int,Vector3D) method");
			rows[i].x = v.x;
			rows[i].y = v.y;
			rows[i].z = v.z;
			rows[i].w = 0;
		}

		/// <summary>
		/// Gets the <see cref = "Vector3D">3d vector</see> that forms to
		/// base vector of the given axis.
		/// </summary>
		/// <param name="i">The index of the axis, where 0 = x,1 = y, 2 = z.
		/// The w part is not indexable</param>
		/// <returns>The base ector of the given axis</returns>
		/// <remarks>
		/// This method threads the matrix as a 3x3 matrix. The other parts
		/// are simply ignored.
		/// </remarks>
		public Vector3D GetBaseVector(int i)
		{
			Debug.Assert(i >= 0 && i < 3, "You can only access the 3x3 part of the matrix with the GetBaseVector(int) method");
			return new Vector3D(rows[i].x,rows[i].y,rows[i].z);
		}

		/// <summary>
		/// Creates an copy of this matrix.
		/// </summary>
		/// <returns>A new matrix that's a copy of this matrix.</returns>
		public Matrix4 Clone()
		{
			Matrix4 ret = new Matrix4();
			ret.rows[0] = rows[0];
			ret.rows[1] = rows[1];
			ret.rows[2] = rows[2];
			ret.rows[3] = rows[3];
			return ret;
		}

		/// <isoverload cref = "Clone"/>
		object ICloneable.Clone()
		{
			return Clone();
		}

		public void ExtractTransformations(out Vector3D translation,
			out EulerRot rotation, out Vector3D scale)
		{
			// TODO: Test this method

			Matrix4 tmp = this.Clone();
			translation.x = tmp.rows[0].w;
			translation.y = tmp.rows[1].w;
			translation.z = tmp.rows[2].w;
			tmp.rows[0].w = tmp.rows[1].w = tmp.rows[2].w = 0;

			// extract the yaw
			Vector3D fwdVec = tmp.rows[0].Get3DPart();
			rotation.yaw = Vector2D.GetAngle(fwdVec.FromTop(), new Vector2D(0, 1));
			// undo the yaw
			tmp.RotateYaw(-rotation.yaw);
			
			// extract the pitch
			Vector3D upVec = tmp.rows[1].Get3DPart();
			rotation.pitch = Vector2D.GetAngle(upVec.FromRight(), new Vector2D(1, 0));
			// undo the pitch
			tmp.RotatePitch(-rotation.pitch);

			// extract the roll
			upVec = tmp.rows[1].Get3DPart();
			rotation.roll = Vector2D.GetAngle(new Vector2D(-upVec.x,upVec.y),
				new Vector2D(0,1));
			// undo the roll
			tmp.RotateRoll(-rotation.roll);

			scale.x = tmp.rows[0].x;
			scale.y = tmp.rows[1].y;
			scale.z = tmp.rows[2].z;
		}
	}
}